package util;

import com.crunch.graphics.gpu.GpuDrawCall;
import com.crunch.graphics.gpu.GpuProgram;
import com.crunch.graphics.gpu.GpuState;
import com.crunch.math.Vector2f;
import com.crunch.math.Vector4f;
import com.crunch.physics.*;
import resources.ResourceManager;

import javax.media.opengl.GL;
import javax.media.opengl.GL3;

/**
 * Renders physics simulation.
 */
public class PhysicsDebugRenderer {
	public PhysicsDebugRenderer(ResourceManager resourceManager) {
		lineProgram = resourceManager.getProgram("line");
		lineMvpMatrixIndex = lineProgram.getUniformIndex("mvpMatrix");
		lineXyxyIndex = lineProgram.getUniformIndex("xyxy");
		lineColorIndex = lineProgram.getUniformIndex("color");

		capsuleProgram = resourceManager.getProgram("capsule");
		capsuleMvpMatrixIndex = capsuleProgram.getUniformIndex("mvpMatrix");
		capsuleXyxyIndex = capsuleProgram.getUniformIndex("xyxy");
		capsuleRadiusIndex = capsuleProgram.getUniformIndex("radius");
		capsuleColorIndex = capsuleProgram.getUniformIndex("color");
	}

	public static final float NORMAL_SCALE = 0.5f;
	public static final float CONTACT_SCALE = 0.25f;
	public static final Vector4f STATIC_COLLIDER_COLOR = new Vector4f(1.0f, 1.0f, 1.0f, 1.0f);
	public static final Vector4f DYNAMIC_ENTITY_COLOR = new Vector4f(0.0f, 1.0f, 0.0f, 1.0f);
	public static final Vector4f MOVABLE_ENTITY_COLOR = new Vector4f(1.0f, 0.0f, 1.0f, 1.0f);
	public static final Vector4f PROXIMITY_ZONE_COLOR = new Vector4f(0.0f, 0.0f, 1.0f, 1.0f);
	public static final Vector4f CONTACT_COLOR = new Vector4f(1.0f, 1.0f, 0.0f, 1.0f);
	public static final Vector4f RAYCAST_COLOR = new Vector4f(1.0f, 0.0f, 0.0f, 1.0f);

	// draw single segment
	public void drawStaticCollider(GpuState gpuState, Camera camera, Segment segment, float scale) {
		gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
		lineProgram.bind();

		// reset to no blending
		gpuState.createBlendState().bind();

		lineProgram.setUniform(lineColorIndex, GL3.GL_FLOAT_VEC4, STATIC_COLLIDER_COLOR.getArray());
		lineProgram.setUniform(lineMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

		GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
		drawCall.setParameters(GL.GL_POINTS, 0, 1);

		float[] xyxy = new float[4];
		Vector2f pA = segment.getA().multiplyAssign(scale);
		Vector2f pB = segment.getB().multiplyAssign(scale);
		Vector2f normal = segment.getNormal().multiplyAssign(scale * NORMAL_SCALE);

		// draw segment
		xyxy[0] = pA.x();
		xyxy[1] = pA.y();
		xyxy[2] = pB.x();
		xyxy[3] = pB.y();
		lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
		drawCall.draw();

		// draw normal
		xyxy[0] = (pA.x() + pB.x()) * 0.5f;
		xyxy[1] = (pA.y() + pB.y()) * 0.5f;
		xyxy[2] = xyxy[0] + normal.x();
		xyxy[3] = xyxy[1] + normal.y();
		lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
		drawCall.draw();
	}

	public void drawRaycast(GpuState gpuState, Camera camera, PhysicsRaycast raycast, PhysicsRaycast.Result result, float scale) {
		if (result.intersectionType == PhysicsRaycast.Result.IntersectionType.NONE) {
			return;
		}

		assert !Float.isInfinite(result.t);

		{ // draw ray line
			// attribute-less rendering
			gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
			lineProgram.bind();

			// reset to no blending
			gpuState.createBlendState().bind();

			lineProgram.setUniform(lineColorIndex, GL3.GL_FLOAT_VEC4, RAYCAST_COLOR.getArray());
			lineProgram.setUniform(lineMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

			GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
			drawCall.setParameters(GL.GL_POINTS, 0, 1);

			float[] xyxy = new float[4];
			Vector2f pA = raycast.ray.getOrigin();
			Vector2f pB = raycast.ray.getDirection().multiplyAssign(result.t).addAssign(pA);
			pA.multiplyAssign(scale);
			pB.multiplyAssign(scale);

			// draw segment
			xyxy[0] = pA.x();
			xyxy[1] = pA.y();
			xyxy[2] = pB.x();
			xyxy[3] = pB.y();
			lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
			drawCall.draw();
		}

		if (raycast.radius != 0.0f) { // ray size
			// attribute-less rendering
			gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
			capsuleProgram.bind();

			// reset to no blending
			gpuState.createBlendState().bind();

			capsuleProgram.setUniform(capsuleColorIndex, GL3.GL_FLOAT_VEC4, RAYCAST_COLOR.getArray());
			capsuleProgram.setUniform(capsuleMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

			GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
			drawCall.setParameters(GL.GL_POINTS, 0, 1);

			float[] xyxy = new float[4];
			float[] radius = new float[1];
			Vector2f p = raycast.ray.getDirection().multiplyAssign(result.t).addAssign(raycast.ray.getOrigin());
			p.multiplyAssign(scale);

			// draw capsule
			xyxy[0] = p.x();
			xyxy[1] = p.y();
			xyxy[2] = p.x();
			xyxy[3] = p.y();
			radius[0] = raycast.radius * scale;
			capsuleProgram.setUniform(capsuleXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
			capsuleProgram.setUniform(capsuleRadiusIndex, GL.GL_FLOAT, radius);
			drawCall.draw();
		}

		{ // contact
			// attribute-less rendering
			gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
			lineProgram.bind();

			// reset to no blending
			gpuState.createBlendState().bind();

			lineProgram.setUniform(lineColorIndex, GL3.GL_FLOAT_VEC4, CONTACT_COLOR.getArray());
			lineProgram.setUniform(lineMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

			GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
			drawCall.setParameters(GL.GL_POINTS, 0, 1);

			float[] xyxy = new float[4];
			Vector2f pA = result.point.multiply(scale);
			Vector2f pB = result.normal.multiply(scale * CONTACT_SCALE).addAssign(pA);

			// draw segment
			xyxy[0] = pA.x();
			xyxy[1] = pA.y();
			xyxy[2] = pB.x();
			xyxy[3] = pB.y();
			lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
			drawCall.draw();
		}
	}

	public void drawPhysicsSimulation(GpuState gpuState, Camera camera, PhysicsSimulation physicsSimulation, float scale) {
		{ // static colliders
			// attribute-less rendering
			gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
			lineProgram.bind();

			// reset to no blending
			gpuState.createBlendState().bind();

			lineProgram.setUniform(lineColorIndex, GL3.GL_FLOAT_VEC4, STATIC_COLLIDER_COLOR.getArray());
			lineProgram.setUniform(lineMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

			GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
			drawCall.setParameters(GL.GL_POINTS, 0, 1);

			float[] xyxy = new float[4];
			for (int i = 0; i < physicsSimulation.getStaticColliderCount(); ++i) {
				Segment segment = physicsSimulation.getStaticCollider(i);
				Vector2f pA = segment.getA().multiplyAssign(scale);
				Vector2f pB = segment.getB().multiplyAssign(scale);
				Vector2f normal = segment.getNormal().multiplyAssign(scale * NORMAL_SCALE);

				// draw segment
				xyxy[0] = pA.x();
				xyxy[1] = pA.y();
				xyxy[2] = pB.x();
				xyxy[3] = pB.y();
				lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
				drawCall.draw();

				// draw normal
				xyxy[0] = (pA.x() + pB.x()) * 0.5f;
				xyxy[1] = (pA.y() + pB.y()) * 0.5f;
				xyxy[2] = xyxy[0] + normal.x();
				xyxy[3] = xyxy[1] + normal.y();
				lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
				drawCall.draw();
			}
		}

		{ // movable entities
			// attribute-less rendering
			gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
			lineProgram.bind();

			// reset to no blending
			gpuState.createBlendState().bind();

			lineProgram.setUniform(lineColorIndex, GL3.GL_FLOAT_VEC4, MOVABLE_ENTITY_COLOR.getArray());
			lineProgram.setUniform(lineMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

			GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
			drawCall.setParameters(GL.GL_POINTS, 0, 1);

			float[] xyxy = new float[4];
			for (MovablePhysicsEntity e : physicsSimulation.getMovableEntities()) {
				Polygon shape = e.getShape();
				float c = (float) Math.cos(e.getRotation());
				float s = (float) Math.sin(e.getRotation());
				Vector2f t = e.getPosition();
				for (int i = 0; i < shape.getVertexCount(); ++i) {
					Vector2f pA = shape.getVertex(i);
					Vector2f pB = shape.getVertex((i+1) % shape.getVertexCount());
					Vector2f normal = shape.getNormal(i);
					pA.set(c*pA.x() - s*pA.y(), s*pA.x() + c*pA.y());
					pB.set(c*pB.x() - s*pB.y(), s*pB.x() + c*pB.y());
					normal.set(c * normal.x() - s * normal.y(), s * normal.x() + c * normal.y());
					pA.addAssign(t).multiplyAssign(scale);
					pB.addAssign(t).multiplyAssign(scale);
					normal.multiplyAssign(scale * NORMAL_SCALE);

					// draw segment
					xyxy[0] = pA.x();
					xyxy[1] = pA.y();
					xyxy[2] = pB.x();
					xyxy[3] = pB.y();
					lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
					drawCall.draw();

					// draw normal
					xyxy[0] = (pA.x() + pB.x()) * 0.5f;
					xyxy[1] = (pA.y() + pB.y()) * 0.5f;
					xyxy[2] = xyxy[0] + normal.x();
					xyxy[3] = xyxy[1] + normal.y();
					lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
					drawCall.draw();
				}
			}
		}

		{ // proximity zones
			{ // rectangles
				// attribute-less rendering
				gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
				lineProgram.bind();

				// reset to no blending
				gpuState.createBlendState().bind();

				lineProgram.setUniform(lineColorIndex, GL3.GL_FLOAT_VEC4, PROXIMITY_ZONE_COLOR.getArray());
				lineProgram.setUniform(lineMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

				GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
				drawCall.setParameters(GL.GL_POINTS, 0, 1);

				float[] xyxy = new float[4];
				for (PhysicsProximityZone e : physicsSimulation.getProximityZones()) {
					if (e.getShape() == PhysicsProximityZone.Shape.RECTANGLE) {
						Vector2f pMin = e.getShapeBoundMin();
						Vector2f pMax = e.getShapeBoundMax();
						pMin.multiplyAssign(scale);
						pMax.multiplyAssign(scale);

						// draw segments
						xyxy[0] = pMin.x();
						xyxy[1] = pMin.y();
						xyxy[2] = pMax.x();
						xyxy[3] = pMin.y();
						lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
						drawCall.draw();

						xyxy[0] = pMin.x();
						xyxy[1] = pMin.y();
						xyxy[2] = pMin.x();
						xyxy[3] = pMax.y();
						lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
						drawCall.draw();

						xyxy[0] = pMax.x();
						xyxy[1] = pMin.y();
						xyxy[2] = pMax.x();
						xyxy[3] = pMax.y();
						lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
						drawCall.draw();

						xyxy[0] = pMin.x();
						xyxy[1] = pMax.y();
						xyxy[2] = pMax.x();
						xyxy[3] = pMax.y();
						lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
						drawCall.draw();
					}
				}
			}
			{ // circle
				// attribute-less rendering
				gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
				capsuleProgram.bind();

				// reset to no blending
				gpuState.createBlendState().bind();

				capsuleProgram.setUniform(capsuleColorIndex, GL3.GL_FLOAT_VEC4, PROXIMITY_ZONE_COLOR.getArray());
				capsuleProgram.setUniform(capsuleMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

				GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
				drawCall.setParameters(GL.GL_POINTS, 0, 1);

				float[] xyxy = new float[4];
				float[] radius = new float[1];
				for (PhysicsProximityZone e : physicsSimulation.getProximityZones()) {
					if (e.getShape() == PhysicsProximityZone.Shape.CIRCLE) {
						Vector2f pCtr = e.getShapeCenter().multiplyAssign(scale);
						float rad = e.getShapeRadius() * scale;

						xyxy[0] = pCtr.x();
						xyxy[1] = pCtr.y();
						xyxy[2] = pCtr.x();
						xyxy[3] = pCtr.y();
						radius[0] = rad;
						capsuleProgram.setUniform(capsuleXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
						capsuleProgram.setUniform(capsuleRadiusIndex, GL.GL_FLOAT, radius);
						drawCall.draw();
					}
				}
			}
		}

		{ // dynamic entities
			// attribute-less rendering
			gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
			capsuleProgram.bind();

			// reset to no blending
			gpuState.createBlendState().bind();

			capsuleProgram.setUniform(capsuleColorIndex, GL3.GL_FLOAT_VEC4, DYNAMIC_ENTITY_COLOR.getArray());
			capsuleProgram.setUniform(capsuleMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

			GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
			drawCall.setParameters(GL.GL_POINTS, 0, 1);

			float[] xyxy = new float[4];
			float[] radius = new float[1];
			for (DynamicPhysicsEntity e : physicsSimulation.getDynamicEntities()) {
				// determine the two capsule points
				float rot = e.getRotation();
				// rotate by 90 degrees CCW; (-y, x) = (-sin, cos)
				Vector2f upVec = new Vector2f((float) -Math.sin(rot), (float) Math.cos(rot));
				Vector2f pA = upVec.multiply(e.getShape().getHeight() * 0.5f).addAssign(e.getPosition());
				Vector2f pB = upVec.multiply(-e.getShape().getHeight() * 0.5f).addAssign(e.getPosition());
				pA.multiplyAssign(scale);
				pB.multiplyAssign(scale);

				// draw capsule
				xyxy[0] = pA.x();
				xyxy[1] = pA.y();
				xyxy[2] = pB.x();
				xyxy[3] = pB.y();
				radius[0] = e.getShape().getRadius() * scale;
				capsuleProgram.setUniform(capsuleXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
				capsuleProgram.setUniform(capsuleRadiusIndex, GL.GL_FLOAT, radius);
				drawCall.draw();
			}
		}

		{ // contacts
			// attribute-less rendering
			gpuState.unbindBuffer(GL.GL_ARRAY_BUFFER);
			lineProgram.bind();

			// reset to no blending
			gpuState.createBlendState().bind();

			lineProgram.setUniform(lineColorIndex, GL3.GL_FLOAT_VEC4, CONTACT_COLOR.getArray());
			lineProgram.setUniform(lineMvpMatrixIndex, GL3.GL_FLOAT_MAT4, camera.getViewProjectionMatrix().getArray());

			GpuDrawCall.DrawCall drawCall = gpuState.createDrawCall();
			drawCall.setParameters(GL.GL_POINTS, 0, 1);

			float[] xyxy = new float[4];
			for (DynamicPhysicsEntity e : physicsSimulation.getDynamicEntities()) {
				Contact c = e.getContactList();
				while (c != null) {
					Vector2f pA = c.getPoint().multiplyAssign(scale);
					Vector2f pB = c.getNormal().multiplyAssign(scale * CONTACT_SCALE).addAssign(pA);

					// draw segment
					xyxy[0] = pA.x();
					xyxy[1] = pA.y();
					xyxy[2] = pB.x();
					xyxy[3] = pB.y();
					lineProgram.setUniform(lineXyxyIndex, GL3.GL_FLOAT_VEC4, xyxy);
					drawCall.draw();

					c = c.getNext();
				}
			}
		}
	}

	private GpuProgram lineProgram;
	private int lineMvpMatrixIndex;
	private int lineXyxyIndex;
	private int lineColorIndex;

	private GpuProgram capsuleProgram;
	private int capsuleMvpMatrixIndex;
	private int capsuleXyxyIndex;
	private int capsuleRadiusIndex;
	private int capsuleColorIndex;
}
